#include <chrono>
#include <functional>
#include <memory>
#include <string>

#include <filesystem>
#include <csignal>
#include <cstdlib>
#include <fstream>
#include <iomanip>  // For std::put_time
#include <cmath>
#include <memory>
#include <algorithm>


#include "rclcpp/rclcpp.hpp"
#include "common_lib/glog/glog.h"
#include "test_nav2_dwb/costmap_queue/costmap_queue.hpp"
#include "test_nav2_dwb/costmap_queue/map_based_queue.hpp"
#include "test_nav2_dwb/costmap_queue/limited_costmap_queue.hpp"

using namespace std;
using namespace std::chrono_literals;

class TestCostmapQueue : public rclcpp::Node{
    public:
        TestCostmapQueue();
        ~TestCostmapQueue() = default;

        // 测试小图
        void testSmallMap();

        // 测试大图
        void testBigMap();

    private:
        std::vector<double> cell_values_;
};

TestCostmapQueue::TestCostmapQueue(): Node("test_costmap_queue"){
    LOG(INFO) << "test_costmap_queue init.";
    testSmallMap();
}


// 测试小图
// 【金山文档 | WPS云文档】 costmap_queue流程图 https://kdocs.cn/l/crjyT3JI4II1
void TestCostmapQueue::testSmallMap(){
    int width = 5;
    int height = 5;
    nav2_costmap_2d::Costmap2D costmap(width, height, 1.0, 0.0, 0.0);
    costmap_queue::CostmapQueue q(costmap);
    cell_values_.clear();
    cell_values_.resize(width * height);
    double unreachable_score = static_cast<double>(cell_values_.size()) + 1.0;
    std::fill(cell_values_.begin(), cell_values_.end(), unreachable_score);

    int count = 0;
    q.enqueueCell(0, 0);
    q.enqueueCell(2, 3);
    while (!q.isEmpty()) {
        costmap_queue::CellData cell = q.getNextCell();
        // double dist = hypot(cell.x_, cell.y_);
        cell_values_[cell.index_] = costmap_queue::CellData::absolute_difference(cell.src_x_, cell.x_) +
            costmap_queue::CellData::absolute_difference(cell.src_y_, cell.y_);
        cout << "[testSmallMap] count: " << count
            << ", x: " << cell.x_
            << ", y: " << cell.y_
            << ", index: " << cell.index_
            << ", sx: " << cell.src_x_
            << ", sy: " << cell.src_y_
            << ", dist: " << cell.distance_ << endl;

        // 打印每步数据
        for(int y = height-1; y >= 0 ; y--){
            for(int x = 0; x < width; x++){
                int index = y * width + x;
                cout << std::fixed << std::setprecision(3) << std::setw(6) << cell_values_[index] << " ";
            }
            cout << endl;
        }  
        count++;
    }

    // 打印序号
    for(int y = height-1; y >= 0 ; y--){
        for(int x = 0; x < width; x++){
            int index = y * width + x;
            cout << std::setw(3) << index << " ";
        }
        cout << endl;
    }    
    
    // 打印曼哈顿距离
    for(int y = height-1; y >= 0 ; y--){
        for(int x = 0; x < width; x++){
            int index = y * width + x;
            // LOG(INFO) << std::fixed << std::setprecision(3)
            //     << "x: " << x
            //     << ", y: " << y
            //     << ", index: " << index
            //     << ", value: " << cell_values_[index];
            cout << std::fixed << std::setprecision(3) << std::setw(6) << cell_values_[index] << " ";
        }
        cout << endl;
    }

    // 20  21  22  23  24 
    // 15  16  17  18  19 
    // 10  11  12  13  14 
    // 5   6   7   8   9 
    // 0   1   2   3   4 

    // 3.000  2.000  1.000  2.000  3.000 
    // 2.000  1.000  0.000  1.000  2.000 
    // 2.000  2.000  1.000  2.000  3.000 
    // 1.000  2.000  2.000  3.000  4.000 
    // 0.000  1.000  2.000  3.000  5.000 
}

// 测试大图
void TestCostmapQueue::testBigMap(){

}


int main(int argc, char * argv[]){
    FLAGS_colorlogtostderr = true;   // log信息区分颜色
    FLAGS_logtostderr = 1;           // 允许log信息打印到屏幕
    google::SetStderrLogging(google::GLOG_INFO);  // 输出log的最低等级是 INFO (可以设置为WARNING或者更高)
    google::InstallFailureSignalHandler();     // 配置安装程序崩溃失败信号处理器
    // google::InstallFailureWriter(&signalHandler);  // 安装配置程序失败信号的信息打印过程，设置回调函数
    google::InitGoogleLogging((const char *)argv[0]);  // 用当前可执行程序初始化glog


    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<TestCostmapQueue>());
    rclcpp::shutdown();
    return 0;
}